#include "modules/MonitorModule/SawtoothWallMonitor.h"
#include "modules/CtrlModule/PassivePlanner/SawtoothWallPlanner.h"
#include "data_struct/base/DataSet.h"
#include "lib/WrapFuncs.h"

namespace behavior_controller {
SawtoothWallMonitor sawtooth_wall_monitor;

void SawtoothWallMonitor::run() {
    double front_dist = 0;
    double front_angl = 0;
    double back_dist = 0;
    double back_angl = 0;

    bool find_front = front_area_detect(front_dist, front_angl);
    bool find_back = back_area_detect(back_dist, back_angl);
    if (find_front && find_back) {
        double dist_diff = front_dist - back_dist;
        double angl_diff = front_angl - back_angl;
        if ((dist_diff > 0.09) && (dist_diff < 0.25) &&                              //阶跃墙面的距离条件
            (angl_diff < 25.0 / 180.0 * M_PI) && (angl_diff > -25.0 / 180.0 * M_PI)) //阶跃墙面的角度条件
        {
            DataSet::ctrl_state = SAWTOOTH_WALL_PLAN;
            std::cout << "sawtooth wall  find !" << std::endl;
        }
    }
}

bool SawtoothWallMonitor::front_area_detect(double &dist, double &angl) {
    int point_count = 0;
    dist = 0;
    angl = 0;
    //获得的激光雷达数据为激光雷达坐标系下的数据，需要转化到机器人坐标系
    PointCloud<double> local_point_cloud;

    for (auto pt : DataSet::scan_point_cloud) {
        PoseF local_point_pose = PoseF(pt, 0).ConvertOriginCur(lidar_pose); //转化到机器人坐标系
        PointF local_point(local_point_pose.GetPoint());

        //Contour front_area = contour_areas.front_area();
        if (contour_areas.front_area().IsPointInShape(local_point)) //判断点是否在轮廓内
        {
            local_point_cloud.emplace_back(local_point);
            point_count++;
        }
    }
    if (point_count > 5) //点数大于5的时候才有计算寻找墙面直线的意义
    {
        double line_k, line_b;
        Funcs::LineFit(local_point_cloud, line_k, line_b); //将轮廓内的点拟合直线
        //计算机器人与直线的距离和角度
        dist = Funcs::DistToLine(line_k, line_b, PoseF(0, 0, 0)); //因为激光点已经转化到机器人坐标系，因此原点就是机器人位置
        angl = std::atan(line_k);
        return true;
    }
    return false;
}

bool SawtoothWallMonitor::back_area_detect(double &dist, double &angl) {
    int point_count = 0;
    dist = 0;
    angl = 0;
    //获得的激光雷达数据为激光雷达坐标系下的数据，需要转化到机器人坐标系
    PointCloud<double> local_point_cloud;

    for (auto pt : DataSet::scan_point_cloud) {
        PoseF local_point_pose = PoseF(pt, 0).ConvertOriginCur(lidar_pose); //转化到机器人坐标系
        PointF local_point(local_point_pose.GetPoint());

        //Contour back_area = contour_areas.back_area();
        if (contour_areas.back_area().IsPointInShape(local_point)) //判断点是否在轮廓内
        {
            local_point_cloud.emplace_back(local_point);
            point_count++;
        }
    }

    if (point_count > 5) //点数大于5的时候才有计算寻找墙面直线的意义
    {
        double line_k, line_b;
        Funcs::LineFit(local_point_cloud, line_k, line_b); //将轮廓内的点拟合直线
        //计算机器人与直线的距离和角度
        dist = Funcs::DistToLine(line_k, line_b, PoseF(0, 0, 0)); //因为激光点已经转化到机器人坐标系，因此原点就是机器人位置
        angl = std::atan(line_k);
        return true;
    }
    return false;
}

} // namespace behavior_controller